/**
  ******************************************************************************
  * File Name          : jy901b_driver.cpp
  * Description       : jy901b_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <jy901b_driver/jy901b_driver.h>

std::string _port;
serial::Serial ser;

float degree2Rad = 3.1415926/180.0;
unsigned char r_buf[1024];
float a[3],w[3],angle[3],h[3];

float init_Az = 0;
unsigned char init_flag = 0;
float mag_dir = 0;
int imu_msg_count_= 0;
double ax_roll_=0;
double  ax_pitch_=0;
double  ax_yaw_ =0;
double  ax_roll_acc_ = 0;
double  ax_pitch_acc_=0;
double  ax_cm_k_ = 0;
sensor_msgs::Imu imu_pub_data_;
sensor_msgs::Imu imu_calib_data_;
ros::Subscriber imu_calib_sub_;
/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  
  * @param  
  * @retval 
  */
void ParseData(char chr)
{
    static char chrBuf[100];
    static unsigned char chrCnt=0;
    signed short sData[4];
    unsigned char i;
    
    time_t now;
    chrBuf[chrCnt++]=chr;
    if (chrCnt<11) return;
    
    if ((chrBuf[0]!=0x55)||((chrBuf[1]&0x50)!=0x50)) 
    {
        //printf("Error:%x %x\r\n",chrBuf[0],chrBuf[1]);
        memcpy(&chrBuf[0],&chrBuf[1],10);
        chrCnt--;
        return;
    }
    
    memcpy(&sData[0],&chrBuf[2],8);
	  switch(chrBuf[1])
	  {
	      case 0x51://线加速度
		      for (i=0;i<3;i++)
          {
              a[i] = (float)sData[i]/32768.0*16.0;        //设置量程16g
              //a[i] = ((float)((int)((a[i]+0.005)*100)))/100;
          } 
			  time(&now);
		      //printf("\r\nT:%s a:%6.3f %6.3f %6.3f ",asctime(localtime(&now)),a[0],a[1],a[2]);
			  break;
		  case 0x52://角速度  三轴陀螺仪
		      for (i=0;i<3;i++)
          {
              w[i] = ((float)sData[i]/32768.0*2000.0)*3.1415926/180.0;    //转化为弧度
              //w[i] = ((float)((int)((w[i]+0.005)*100)))/100;
          } 
			  //printf("w:%7.3f %7.3f %7.3f ",w[0],w[1],w[2]);					
			  break;
		  case 0x53://角度
		      for (i=0;i<3;i++) 
          {
              angle[i] = (float)sData[i]/32768.0*180.0;
              //angle[i] = ((float)((int)((angle[i]+0.005)*100)))/100;
          }
          switch(init_flag)
          {
              case 10:
                mag_dir = init_Az/10; 
                init_flag++;
                break; 
              case 11:
                angle[2] -= mag_dir; 
                break;             
              default:
                init_Az += angle[2];
                init_flag++;
          }
			  //printf("A:%7.3f %7.3f %7.3f dir:%7.3f\r\n",angle[0],angle[1],angle[2],mag_dir);
			  break;
		  case 0x54://磁场
			  for (i=0;i<3;i++)
        {
            h[i] = (float)sData[i];
        }
		    //printf("h:%4.0f %4.0f %4.0f \r\n",h[0],h[1],h[2]);
			  break;
		  }		
		  chrCnt=0;		
}

//imu校准回调函数
void calib_imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
	imu_calib_data_.header = msg->header;
	imu_calib_data_.angular_velocity = msg->angular_velocity;
	imu_calib_data_.linear_acceleration = msg->linear_acceleration;
}

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "imu_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);   //10hz
    sensor_msgs::Imu imu_raw_data;
    sensor_msgs::Imu imu_do_calib;
    
    sensor_msgs::MagneticField imu_mag_data;
    unsigned char  data_size;
    geometry_msgs::Quaternion q;
    float  last_w = 0;
    float  curr_w = 0;
    int count = 0;

    nh.getParam("/jy901b_driver/port", _port);
    nh.param<double>("ax_cm_k",ax_cm_k_,0.08);      //by weiyang
    ros::Publisher imu_raw_pub = nh.advertise<sensor_msgs::Imu>("imu_raw", 100);
    ros::Publisher imu_raw_pub_do_calib = nh.advertise<sensor_msgs::Imu>("imu_do_calib", 100);
    ros::Publisher imu_mag_pub = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 100);
    imu_calib_sub_ = nh.subscribe<sensor_msgs::Imu>("/imu",10,&calib_imu_callback); 
    

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(9600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    }

    while(ros::ok())
    {
        if(data_size = ser.available()){
            ser.read(r_buf, data_size);
            for (int i=0;i<data_size;i++) {
              //fprintf(fp,"%2X ",r_buf[i]);
              ParseData(r_buf[i]);
            }
        }
        
        
        imu_do_calib.header.stamp = ros::Time::now();
        imu_do_calib.header.frame_id = "imu_link";

        if(mag_dir)
        {
          // q = tf::createQuaternionMsgFromRollPitchYaw(angle[0]*degree2Rad, angle[1]*degree2Rad, angle[2]*degree2Rad);
          q = tf::createQuaternionMsgFromYaw(angle[2]*degree2Rad);

          imu_do_calib.orientation.x = q.x;
          imu_do_calib.orientation.y = q.y;
          imu_do_calib.orientation.z = q.z;
          imu_do_calib.orientation.w = q.w;

          imu_do_calib.angular_velocity.x = w[0];
          imu_do_calib.angular_velocity.y = w[1];
          imu_do_calib.angular_velocity.z = w[2];
          imu_do_calib.linear_acceleration.x = a[0]*9.8;
          imu_do_calib.linear_acceleration.y = a[1]*9.8;
          imu_do_calib.linear_acceleration.z = a[2]*9.8;

         imu_do_calib.orientation_covariance = {0.05, 0, 0,
	    				    0, 0.05, 0,
					    0, 0, 0.05};
          imu_do_calib.angular_velocity_covariance = {0.01, 0, 0,
                            0, 0.01, 0,
                        0, 0, 0.01};
          imu_do_calib.linear_acceleration_covariance = {0, 0, 0,
                                0, 0, 0,
                            0, 0, 0};

          imu_raw_pub_do_calib.publish(imu_do_calib);
        }
  
  
 if(imu_msg_count_<120)       //等待IMU稳态
    {
	    imu_msg_count_++;
	    ax_roll_=0;
	    ax_pitch_=0;
	    ax_yaw_=0;
    }
    else
    {
    ax_roll_acc_ = -(atan2(imu_calib_data_.linear_acceleration.x,imu_calib_data_.linear_acceleration.z));
    ax_roll_ = ax_cm_k_*ax_roll_acc_+(1-ax_cm_k_)*(ax_roll_+(imu_calib_data_.angular_velocity.y)*0.01);
    ax_pitch_acc_ = -(atan2(imu_calib_data_.linear_acceleration.y,imu_calib_data_.linear_acceleration.z));
    ax_pitch_ = ax_cm_k_*ax_pitch_acc_+(1-ax_cm_k_)*(ax_pitch_+(imu_calib_data_.angular_velocity.x)*0.01);
    ax_yaw_ = (ax_yaw_ -(imu_calib_data_.angular_velocity.z)*0.01);
    }
    // std::cout<<"roll:  "<<ax_roll_<<"  pitch:  "<<ax_pitch_<<"  yaw:  "<<ax_yaw_*180/3.1415926<<std::endl;


    imu_raw_data.orientation = tf::createQuaternionMsgFromRollPitchYaw(ax_roll_,ax_pitch_,ax_yaw_);
    imu_raw_data.header.stamp = ros::Time::now();
    imu_raw_data.header.frame_id =  "imu_link";
    imu_raw_data.angular_velocity.x = w[0];
    imu_raw_data.angular_velocity.y = w[1];
    imu_raw_data.angular_velocity.z = w[2];
    imu_raw_data.linear_acceleration.x = a[0];
    imu_raw_data.linear_acceleration.y = a[1];
    imu_raw_data.linear_acceleration.z = a[2];
    //imu_pub_data_.orientation.w = imu_data_.q0;
    //imu_pub_data_.orientation.x = imu_data_.q1;
    //imu_pub_data_.orientation.y = imu_data_.q2;
    //imu_pub_data_.orientation.z = imu_data_.q3;

    imu_raw_data.orientation_covariance = {0.05, 0, 0,
	    				    0, 0.05, 0,
					    0, 0, 0.05};
    imu_raw_data.angular_velocity_covariance = {0.01, 0, 0,
	    				         0, 0.01, 0,
					         0, 0, 0.01};
    imu_raw_data.linear_acceleration_covariance = {0, 0, 0,
	    				             0, 0, 0,
					             0, 0, 0};

    imu_raw_pub.publish(imu_raw_data);


/*        imu_raw_data.angular_velocity.x = 0;//w[0];
        imu_raw_data.angular_velocity.y = 0;//w[1];
        imu_raw_data.angular_velocity.z = last_w;//w[2];
   

        imu_raw_data.linear_acceleration.x = a[0];
        imu_raw_data.linear_acceleration.y = 0;//a[1];
        imu_raw_data.linear_acceleration.z = a[2]*9.8;
        //  imu_raw_data.linear_acceleration.z = a[2];
    
        imu_raw_pub.publish(imu_raw_data);
*/
        imu_mag_data.header.stamp = ros::Time::now();
        imu_mag_data.header.frame_id = "mag_link";

        imu_mag_data.magnetic_field.x = h[0];
        imu_mag_data.magnetic_field.y = h[1];
        imu_mag_data.magnetic_field.z = h[2];

        imu_mag_pub.publish(imu_mag_data);

        ros::spinOnce();
        loop_rate.sleep();
    }

    ser.close(); 
    return 0;
}
